#include "Armor.h"

ArmorParam armorParam = ArmorParam();

ArmorDetector::ArmorDetector()
{
    states = LIGHTS_NOT_FOUND;
}

ArmorDetector::~ArmorDetector()
{

}

void ArmorDetector::resetDetector()
{
    states = LIGHTS_NOT_FOUND;
    lights.clear();  //清空vector
    //armors.clear();
}

void ArmorDetector::setEnemyColor(Color enemyColor)
{
    this->enemyColor = enemyColor;
}

void ArmorDetector::PreprocessImg(Mat &src)
{
    src.copyTo(srcImg);
    srcImg_binary = Mat::zeros(srcImg.size(), CV_8UC1);

    uchar *pdata = (uchar*)srcImg.data;
    uchar *qdata = (uchar*)srcImg_binary.data;
    int srcdata = srcImg.cols * srcImg.rows;

    if (enemyColor == RED)
    {
        for(int i=0; i<srcdata; ++i)
        {
            if (*(pdata+2) - *pdata > armorParam.color_threshold)
                *qdata = 255;
            pdata += 3;
            qdata ++;
        }
    }
    else if (enemyColor == BLUE)
    {
        for(int i=0; i<srcdata; ++i)
        {
            if (*pdata - *(pdata+2) > armorParam.color_threshold)
                *qdata = 255;
            pdata += 3;
            qdata ++;
        }
    }

    Mat kernel = getStructuringElement(MORPH_ELLIPSE, Size(3, 3));
    dilate(srcImg_binary, srcImg_binary, kernel);
}

void ArmorDetector::run(Mat &src)
{
    this->PreprocessImg(src);

    resetDetector();

    findLights();

    if(!states == LIGHTS_FOUND)
    {
        matchArmors();

        if(!states == ARMOR_FOUND)
        {
            setTargetArmor();
        }
    }
}

void showLights(Mat & image, const vector<LightBar> & lights)
{
    Mat lightDisplay;//image for the use of dialaying the lights 显示灯条用的图像
    image.copyTo(lightDisplay);//get a copy of srcImg 获取源图像的拷贝
    //if detector finds lights 如果找到了灯条
    if (!lights.empty())
    {
        putText(lightDisplay, "LIGHTS FOUND!", Point(100, 50), FONT_HERSHEY_SIMPLEX, 1, Scalar(0, 255, 255), 1, 8, false); //title LIGHT_FOUND 大标题 “找到了灯条”
        for (auto light : lights)
        {
            Point2f lightVertices[4];
            light.lightRect.points(lightVertices);
            //draw all the lights' contours 画出所有灯条的轮廓
            for (size_t i = 0; i < 4; i++)
            {
                line(lightDisplay, lightVertices[i], lightVertices[(i + 1) % 4], Scalar(255, 0, 255), 1, 8, 0);
            }

            //draw the lights's center point 画出灯条中心
            circle(lightDisplay, light.center, 2, Scalar(0, 255, 0), 2, 8, 0);

            //show the lights' center point x,y value 显示灯条的中心坐标点
            putText(lightDisplay, to_string(int(light.center.x)), light.center, FONT_HERSHEY_PLAIN, 1, Scalar(0, 255, 0), 1, 8, false);
            putText(lightDisplay, to_string(int(light.center.y)), light.center + Point2f(0, 15), FONT_HERSHEY_PLAIN, 1, Scalar(0, 255, 0), 1, 8, false);
        }
    }
    //if detector does not find lights 如果没找到灯条
    else
    {
        putText(lightDisplay, "LIGHTS NOT FOUND!", Point(100, 50), FONT_HERSHEY_SIMPLEX, 1, Scalar(0, 0, 255), 1, 8, false);//title LIGHT_NOT_FOUND 大标题 “没找到灯条”
    }
    //show the result image 显示结果图
    imshow("Lights Monitor", lightDisplay);
}

void showArmors(Mat & image, const vector<ArmorBox> & armors, const ArmorBox & targetArmor)
{
    Mat armorDisplay; //Image for the use of displaying armors 展示装甲板的图像
    image.copyTo(armorDisplay); //get a copy of srcImg 源图像的拷贝
    // if armors is not a empty vector (ARMOR_FOUND) 如果找到了装甲板
    if (!armors.empty())
    {
        putText(armorDisplay, "ARMOR FOUND!", Point(100, 50), FONT_HERSHEY_SIMPLEX, 1, Scalar(255, 255, 0), 1, 8, false); //title FOUND 大标题 “找到了装甲板”
        //draw all the armors' vertices and center 画出所有装甲板的顶点边和中心
        for (auto armor : armors)
        {
            //draw the center 画中心
            circle(armorDisplay, armor.center, 2, Scalar(0, 255, 0), 2);
            for (size_t i = 0; i < 4; i++)
            {
                line(armorDisplay, armor.armorVertices[i], armor.armorVertices[(i + 1) % 4], Scalar(255, 255, 0), 2, 8, 0);
            }
            //display its center point x,y value 显示中点坐标
            putText(armorDisplay, to_string(int(armor.center.x)), armor.center, FONT_HERSHEY_PLAIN, 1, Scalar(255, 0, 255), 1, 8, false);
            putText(armorDisplay, to_string(int(armor.center.y)), armor.center + Point2f(0, 15), FONT_HERSHEY_PLAIN, 1, Scalar(255, 0, 255), 1, 8, false);
            //putText(armorDisplay, to_string(int(armor.armorNum)), armor.center + Point2f(15, 30), FONT_HERSHEY_PLAIN, 1, Scalar(255, 255, 255), 1, 8, false);
        }
        //connect all the vertices to be the armor contour 画出装甲板轮廓
        for (size_t i = 0; i < 4; i++)
        {
            line(armorDisplay, targetArmor.armorVertices[i], targetArmor.armorVertices[(i + 1) % 4], Scalar(255, 255, 255), 2, 8, 0);
        }
    }
    //if armors is a empty vector (ARMOR_NOT FOUND) 如果没找到装甲板
    else
    {
        putText(armorDisplay, "ARMOR NOT FOUND!", Point(100, 50), FONT_HERSHEY_SIMPLEX, 1, Scalar(255, 0, 255), 1, 8, false);//title NOT FOUND 大标题 “没找到装甲板”
    }
    //show the result armors image 显示结果图
    imshow("Armor Monitor", armorDisplay);
}

void ArmorDetector::showDebugInfo(bool showSrcImg_ON, bool showSrcBinary_ON, bool showLights_ON, bool showArmors_ON, bool textLights_ON, bool textArmors_ON, bool textScores_ON)
{
    if (showSrcImg_ON)
        imshow("srcImg", srcImg);
    if (showSrcBinary_ON)
        imshow("srcImg_Binary", srcImg_binary);
    if (showLights_ON)
        showLights(srcImg, lights);
    if (showArmors_ON)
        showArmors(srcImg, armors, targetArmor);
    if (textLights_ON){}
        //textLights(lights);
    if (textArmors_ON){}
        //textArmors(armors);
    if (textScores_ON){}
        //textScores(armors, lastArmor);
}
